
#include "skeletonsMatcher.h"

Number length(const vector<Point3> &branch, int lim=-1);
Point3 getMappedPoint(const vector<Point3> &branch, Number t, Number &tnew, int &j);
void fitBranch(const vector<Point3> &source, const vector<Point3> &target, 
            vector<Point3> &presult, vector<pair<Number, int> > &tresult);

/// Busca dentro de los branches un branch que comienza con b y temina con e
/// Si el branch es encontrado returna el id y f=FALSE
/// f sera TRUE si existe el branch pero en oreden contrario(comenzando en e y terminando en b)
void getBranchId(const vector<vector<int> > &branches, int b, int e, int &id, bool &f);

SkeletonsMatcher::SkeletonsMatcher() {
   gskel = 0;
   uskel = 0;
}

SkeletonsMatcher::SkeletonsMatcher(const SkeletonsMatcher& m) {
   
   gskel = m.gskel;
   uskel = m.uskel;
   gbranches = m.gbranches;
   ubranches = m.ubranches;
   joints = m.joints;
}

SkeletonsMatcher::SkeletonsMatcher(GSkeleton *g, GSkeleton *u) {
   gskel = g;
   uskel = u;
   
   int nmax = (gskel->nBones()<uskel->nBones())? gskel->nBones():uskel->nBones();
   gbranches.reserve(nmax);
   ubranches.reserve(nmax);
}

void SkeletonsMatcher::genBranchesRelation() {
   
   cout<<"genBranchesRelation() begin ..."<<endl;
   
   gbranches = gskel->branches; /// ASF skeleton
   ubranches = uskel->branches; /// SKE skeleton
   
   //~ cout<<"ubranches before"<<endl;
   //~ for (uint i=0; i<ubranches.size(); ++i) {
      //~ cout<<" "<<i<<" : " ;
      //~ copy(ubranches[i].begin(), ubranches[i].end(), ostream_iterator<int>(cout, " "));
      //~ cout<<endl;
   //~ }
   
   //~ cout<<"gbranches.size(): "<<gbranches.size()<<endl;
   //~ cout<<"gbranches"<<endl;
   //~ for (uint i=0; i<gbranches.size(); ++i) {
      //~ cout<<" "<<i<<" : " ;
      //~ copy(gbranches[i].begin(), gbranches[i].end(), ostream_iterator<int>(cout, " "));
      //~ cout<<endl;
   //~ }
   
   cout<<"filename: "<<gskel->filename<<endl;
   assert(gskel->filename.size() > gskel->filename.find("01.asf"));
   //~ string command = "skelpy/./matchskel.py " + 
                     //~ gskel->filename + " " + 
                     //~ uskel->filename + " > relation.txt";
   string command = "skelpy/./matchskel.py data/01.ske " + 
                     uskel->filename + " > relation.txt";
   
   cout<<command<<endl;
   cout<<"calling skelpy..."<<endl;
   system(command.c_str());
   cout<<"skelpy ... done! "<<endl;
   
   /// Call the python program
   map<int, int> jrelation; // joints relation map
   int a,b;
   ifstream relationfile("relation.txt");
   if (not relationfile.is_open()) {
      cout<<"skelpy error: dont exist the relation file!!!"<<endl;
      assert(false);
   }
   while(not relationfile.eof()) {
      relationfile>>a>>b;
      jrelation[a] = b;
   }
   
   jrelation[19] = 19;
   jrelation[22] = 22;
   jrelation[10] = 10;
   jrelation[18] = 18;
   jrelation[0] = 0;
   jrelation[13] = 13;
   jrelation[3] = 3;
   
   /// Cuidar que el orden de las secuencias de joints que componen un branch sea el 
   /// el mismo en ambos esqueletos
   for (int i=0; i<gbranches.size(); ++i) {
      const int &begin = gbranches[i].front();
      const int &end = gbranches[i].back();
      int j=-1; // the position of the branch
      bool isreverse = false;
      getBranchId(ubranches, jrelation[begin], jrelation[end], j, isreverse);
      assert(j != -1);
      vector<int> branch = ubranches[i];
      if (isreverse) ubranches[i] = vector<int>(ubranches[j].rbegin(), ubranches[j].rend());
      else           ubranches[i] = ubranches[j];
      if (i != j)    ubranches[j] = branch;
   }
   
   //~ cout<<"ubranches after"<<endl;
   //~ for (uint i=0; i<ubranches.size(); ++i) {
      //~ cout<<" "<<i<<" : " ;
      //~ copy(ubranches[i].begin(), ubranches[i].end(), ostream_iterator<int>(cout, " "));
      //~ cout<<endl;
   //~ }
}

void SkeletonsMatcher::fitBranches() {
   
   // assert(gbranches.size() == ubranches.size());
   joints.resize(uskel->nJoints(), Point3(0,0,0));
   vector<pair<Number, int> > params(uskel->nJoints(), make_pair(0.0,-1));
   
   for (uint i=0; i<gbranches.size(); ++i) {
      vector<Point3> source, target;
      const vector<int> &branchsource = ubranches[i];
      const vector<int> &branchtarget = gbranches[i];
      assert(not branchsource.empty());
      assert(not branchtarget.empty());
      
      for (uint s=0; s<branchsource.size(); ++s)
         source.push_back(uskel->getJointPos(branchsource[s]));
      for (uint t=0; t<branchtarget.size(); ++t)
         target.push_back(gskel->getJointPos(branchtarget[t]));
      
      vector<Point3> presult(source.size(), Point3(0,0,0)); // positions result
      vector<pair<Number, int> > tresult(source.size()); // parameters result
      
      fitBranch(source, target, presult, tresult);
      /// Apply the mapping into the skeleton without scale nor translation
      if (params[branchsource[0]].second == -1) { // protect a correct joint positioning made before
         joints[branchsource[0]] = source[0];
         params[branchsource[0]] = make_pair(tresult[0].first, branchtarget[tresult[0].second]);
      }
      for (uint s=1; s<branchsource.size(); ++s) {
         Vector3 br;
         if (s == branchsource.size()-1)
            br = target[s] - target[s-1]; // get the direction of the last original bone (for feet case)
         else
            br = presult[s] - presult[s-1]; // bone result
         br = br/sqrt(br*br); // bone result direction
         Vector3 bs = source[s] - source[s-1]; // bone source
         Number normabs = sqrt(bs*bs); // bone source norma
         joints[branchsource[s]] = joints[branchsource[s-1]] + br*normabs;
         params[branchsource[s]] = make_pair(tresult[s].first, branchtarget[tresult[s].second]);
      }
      
      /// Fit the source skeleton over the target skeleton
      //~ for (uint s=0; s<branchsource.size(); ++s) {
         //~ joints[branchsource[s]] = presult[s];
         //~ params[branchsource[s]] = make_pair(tresult[s].first, branchtarget[tresult[s].second]);
      //~ }
   }
   
   // fix unjoined branches
   for (uint i=gbranches.size(); i<ubranches.size(); ++i) {
      const vector<int> &ubranch = ubranches[i];
      const pair<Number, int> &relatedParam = params[ubranch[0]];
      assert(relatedParam.second != -1);
      const Vector3 t = joints[ubranch[0]] - uskel->getJointPos(ubranch[0]);
      for (uint j=1; j<ubranch.size(); ++j) {
         params[ubranch[j]] = relatedParam;
         joints[ubranch[j]] = uskel->getJointPos(ubranch[j]) + t;
      }
   }
   
   // update the joint positions of the skeleton
   for (int i=0; i<uskel->nJoints(); ++i) {
      const pair<Number, int> &param = params[i];
      assert(param.second != -1);
         uskel->setRelatedParamBone(i, param);
         uskel->setJointPos(i, joints[i]);
   }
   //~ cout<<"printandooooooooooooooo"<<endl;
   //~ for (int i=0; i<uskel->nJoints(); ++i) {
      //~ const pair<Number, int> &param = uskel->getRelatedParamBone(i);
      //~ cout<<param.first<<" <:> "<<param.second<<endl;
   //~ }
   
   
   /// leva o gskeleton sobre o uskeleton
   /*
   assert(gbranches.size() == ubranches.size());
   joints.resize(gskel->nJoints(), Point3(0,0,0));
   
   for (uint i=0; i<gbranches.size(); ++i) {
      vector<Point3> source, target;
      const vector<int> &branchsource = gbranches[i];
      const vector<int> &branchtarget = ubranches[i];
      for (int s=0; s<branchsource.size(); ++s)
         source.push_back(gskel->getJointPos(branchsource[s]));
      for (int t=0; t<branchtarget.size(); ++t)
         target.push_back(uskel->getJointPos(branchtarget[t]));
      
      vector<Point3> result(source.size(), Point3(0,0,0)); // default values
      vector<pair<Number, int> > tresult(source.size()); // parameters result
      
      fitBranch(source, target, result, tresult);
      for (int s=0; s<branchsource.size(); ++s)
         joints[branchsource[s]] = result[s];
   }
   
   // update the joint positions of the skeleton
   for (int i=0; i<gskel->nJoints(); ++i) {
      gskel->setJointPos(i, joints[i]);
   }
   */
   
}

void fitBranch(const vector<Point3> &source, const vector<Point3> &target, 
            vector<Point3> &presult, vector<pair<Number, int> > &tresult) {
   
   Number l = length(source);
   for (uint i=0; i<source.size(); ++i) {
      Number t = length(source, i)/l;
      Number tnew; int j;
      presult[i] = getMappedPoint(target, t, tnew, j);
      tresult[i] = make_pair(tnew, j);
   }
}

Point3 getMappedPoint(const vector<Point3> &branch, Number t, Number &tnew, int &j) {
   
   Number tlast = 0.0;
   Number l = length(branch);
   for (uint i=1; i<branch.size(); ++i) {
      j = i;
      Number ti = length(branch, i)/l;
      if (ti>=t) {
         tnew = (t-tlast)/(ti-tlast);
         return branch[i-1] + tnew*(branch[i]-branch[i-1]);
      }
      else tlast = ti;
   }
   
   tnew = t;
   return branch.back();
}

Number length(const vector<Point3> &branch, int lim) {
   
   uint limit = branch.size()-1;
   if (lim != -1) limit = lim;
   
   Number sum = 0;
   for (uint i=1; i<=limit; ++i) {
      Vector3 v(branch[i-1], branch[i]);
      sum += sqrt(v*v);
   }
   return sum;
}

void getBranchId(const vector<vector<int> > &branches, int begin, int end, int &id, bool &isreverse) {
   
   for (int i=0; i<branches.size(); ++i) {
      const vector<int> &branch = branches[i];
      if (begin == branch.front() and end == branch.back()) {
         id = i;
         return;
      }
   }
   
   for (int i=0; i<branches.size(); ++i) {
      const vector<int> &branch = branches[i];
      if (end == branch.front() and begin == branch.back()) {
         id = i;
         isreverse = true;
         return;
      }
   }
   
   id = -1;
   return;
}
